import rclpy
import numpy as np
from rclpy.node import Node
from unitree_go.msg import LowState
import random


class LowStatePublisher(Node):
    def __init__(self):
        super().__init__('lowstate_publisher')
        self.publisher_ = self.create_publisher(LowState, '/lowstate', 10)
        self.timer = self.create_timer(0.1, self.publish_msg)  # 10Hz发布频率

    def publish_msg(self):
        msg = LowState()
        # 头部信息
        msg.head = [0xFE, 0xEF]
        msg.version = [1, 0]

        # IMU数据
        msg.imu_state.quaternion = [float(0), float(0), float(0), float(1)]  # 四元数
        msg.imu_state.gyroscope = [float(0.1), float(-0.1), float(0.05)]  # 陀螺仪

        # 电机数据（填充前12个）
        for i in range(12):
            msg.motor_state[i].q = float(random.uniform(-3.14, 3.14))
            msg.motor_state[i].dq = 0.0
            msg.motor_state[i].tau_est = 0.0

        self.publisher_.publish(msg)
        self.get_logger().info(f'Published LowState tick={msg.tick}')


def main(args=None):
    rclpy.init(args=args)
    node = LowStatePublisher()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()